#include <opencv2/opencv.hpp>
#include <iostream>

using namespace cv;
using namespace std;

bool isPaused = false;
bool stopProgram = false;

void onKeyPressed(int key) {
    if (key == 'k') {  // 暂停视频
        isPaused = true;
    } else if (key == 'q') {  // 继续播放视频
        isPaused = false;
    } else if (key == 'p') {  // 结束程序
        stopProgram = true;
    }
}

int main()
{
    VideoCapture cap("/home/lwj/danmu.cv/liang .avi");  // 打开视频文件

    if (!cap.isOpened()) {
        cout << "无法打开视频文件" << endl;
        return -1;
    }

    Mat frame, gray, binary;
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;

    while (true) {
        if (stopProgram) {
            break;
        }

        if (!isPaused) {
            cap >> frame;  // 读取每一帧视频

            if (frame.empty()) {
               cout << " 未能读取帧" << endl;
               break;
            }
            // 高斯滤波 
            GaussianBlur(frame, frame, Size(5, 5), 0); 

            // 将帧转换成灰度图像
            cvtColor(frame, gray, COLOR_BGR2GRAY);

            // 开运算
           Mat kernel = getStructuringElement(MORPH_RECT, Size(5, 5)); 
           morphologyEx(gray, gray, MORPH_OPEN, kernel); 
        
            dilate(gray,gray,kernel,Point(-1,-1),3);//膨胀
        
            threshold(gray, binary, 220, 255, THRESH_BINARY);//二值化

            // 查找轮廓
            findContours(binary, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);
            
            // 寻找包围轮廓的最小矩形
            vector<vector<Point>> contours_poly(contours.size());
            vector<Rect> boundRect(contours.size());
            Point2f center;
            float radius;
        int contourIndex = -1;
        double prev_angle = 0.0;
        double prev_time = 0.0;
        Point2f prev_center;

for (size_t i = 0; i < contours.size(); i++) {
    double area = contourArea(contours[i]);
    if (area > 3000) {
        contourIndex = i;
        break;
    }
}

// 绘制面积大于 3000 的轮廓
if (contourIndex != -1) {
    cv::Moments moments = cv::moments(contours[contourIndex]);
    Point2f center(moments.m10 / moments.m00, moments.m01 / moments.m00);
    circle(frame, center, 5, Scalar(0, 0, 255), -1);
    drawContours(frame, contours, contourIndex, Scalar(0, 0, 255), 2); // 绘制红色轮廓


    for (size_t i = 0; i < contours.size(); i++) {
        // 计算轮廓的面积和最小边界矩形
        double area = contourArea(contours[i]);
        cv::RotatedRect rect = cv::minAreaRect(contours[i]);
        float aspectRatio = rect.size.width / rect.size.height;

        if ((area < 1250) && (area > 1050) && (aspectRatio >= 0.9 && aspectRatio <= 1.1)) {
            // 绘制近似为正方形且面积小于1500的轮廓
            drawContours(frame, contours, static_cast<int>(i), cv::Scalar(255, 0, 0), 2);

            // 获取中心点坐标
            Point2f center = rect.center;

            // 更新先前中心点
            prev_center = center;
        }
    }
        // 连接两点
    if (prev_center!=Point2f()&& center!=Point2f()) {
        line(frame, prev_center, center, Scalar(255, 255, 0), 2);

        // 端点可视化
        circle(frame, prev_center, 5, Scalar(255, 255, 0), -1); // 蓝色
        circle(frame, center, 5, Scalar(255, 255, 0), -1); // 蓝色
    }
    else
    {
        cout<<"找不到点"<<endl;
    }

        // 计算角速度
    if (prev_center!=Point2f()) {
        double dx = center.x - prev_center.x;
        double dy = center.y - prev_center.y;
        double angle = atan2(dy, dx);
        double curr_time = cv::getTickCount();//设置时间戳
        double dt = (curr_time - prev_time) / cv::getTickFrequency();
        double angular_velocity = std::abs((angle - prev_angle) / dt);
        
        // 输出角速度
        std::ostringstream text;
        text << "angular:(rad/s) " << angular_velocity;
        cv::putText(frame, text.str(), cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 255),1);

        // 更新先前值
        prev_angle = angle;
        prev_time = curr_time;
    }
}

        cv::circle(binary, prev_center, 180, cv::Scalar(0, 0, 0), -1); // 在prev_center位置绘制半径为180的黑色实心圆
            // 查找轮廓
            findContours(binary, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);

         Point2i pixelCorners[4];
            for (size_t i = 0; i < contours.size(); i++) {
                //计算矩形
                RotatedRect rect = minAreaRect(contours[i]); 
                float aspectRatio = rect.size.width / rect.size.height;
                double area = contourArea(contours[i]); 
                if (area > 3500) {
                    if (aspectRatio >= 0.4 && aspectRatio <= 2.3) {
                approxPolyDP(contours[i], contours_poly[i], 3, true);
                boundRect[i] = boundingRect(contours_poly[i]);


                // 获取最小矩形的四个角点坐标
                Point2f corners[4];
                rect.points(corners); 

                // 绘制最小矩形的四条边
                line(frame, corners[0], corners[1], Scalar(0, 255, 0), 2); // 左上角到左下角
                line(frame, corners[1], corners[2], Scalar(0, 255, 0), 2); // 左下角到右下角
                line(frame, corners[2], corners[3], Scalar(0, 255, 0), 2); // 右下角到右上角
                line(frame, corners[3], corners[0], Scalar(0, 255, 0), 2); // 右上角到左上角

               
                // 将浮点数坐标四舍五入为整数
                for (int i = 0; i < 4; i++) {
                    pixelCorners[i] = Point2i(cvRound(corners[i].x), cvRound(corners[i].y));
                }

                // 在图像上绘制四个角点
                for (int i = 0; i < 4; i++) {
                    circle(frame, pixelCorners[i], 5, Scalar(0, 0, 255), -1); 
                }

                // 在图像上连接角点以显示最小矩形的四条边
                for (int i = 0; i < 4; i++) {
                    int next = (i + 1) % 4;
                    line(frame, pixelCorners[i], pixelCorners[next], Scalar(0, 255, 0), 2);
                }

                // 输出四个角点的像素值
                for (int i = 0; i < 4; i++) {
                    std::string text = "Point " + std::to_string(i) + ": (" + std::to_string(pixelCorners[i].x) + ", " + std::to_string(pixelCorners[i].y) + ")";
                    putText(frame, text, Point(pixelCorners[i].x, pixelCorners[i].y - 10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), 1);
                }

                    }
                }
            }

                    // 将角点像素坐标传递给距离测量代码
                    vector<Point2d> image_points;
                    // 将输出的坐标点插入到 image_points 中
                image_points.push_back(Point2d(pixelCorners[0].x, pixelCorners[0].y)); // 左上角
                image_points.push_back(Point2d(pixelCorners[2].x, pixelCorners[2].y)); // 右下角
                image_points.push_back(Point2d(pixelCorners[1].x, pixelCorners[1].y)); // 右上角
                image_points.push_back(Point2d(pixelCorners[3].x, pixelCorners[3].y)); // 左下角

                    // 画出四个特征点
                    for (int i = 0; i < image_points.size(); i++)
                    {
                    circle(frame, image_points[i], 3, Scalar(0, 0, 255), -1);
                    }

                    // 3D 特征点世界坐标，与像素坐标对应，单位是mm
                    std::vector<Point3d> model_points;
                    model_points.push_back(Point3d(-186.0f, -191.0f, 0)); // 左上角(-42.5mm,-42.5mm)
                    model_points.push_back(Point3d(+186.0f, +191.0f, 0));
                    model_points.push_back(Point3d(+186.0f, -191.0f, 0));
                    model_points.push_back(Point3d(-186.0f, +191.0f, 0));

                    // 相机内参矩阵
                    Mat camera_matrix = (Mat_<double>(3, 3) << 2447.455952, 0.000000, 665.663057,
                    0.000000,2516.831547, 595.822693,0.000000, 0.000000, 1.000000);
                    // 相机畸变系数
                    Mat dist_coeffs = (Mat_<double>(5, 1) << -0.022395, -0.086252, 
                    0.002104, -0.012177, 0.000000);

                    // 旋转向量
                    Mat rotation_vector;
                    // 平移向量
                    Mat translation_vector;

                    /// pnp求解
                    bool solve_success = solvePnP(model_points, image_points, camera_matrix, dist_coeffs, 
                                        rotation_vector, translation_vector, 0, SOLVEPNP_ITERATIVE);

                    if (solve_success) {
                        double depth = translation_vector.at<double>(2); // 获取Z轴位移作为深度信息/距离
                
                        // 在图像中心显示深度/距离信息
                        Point center(frame.cols / 2, frame.rows / 2);
                        putText(frame, "Depth: " + to_string(depth) + " mm", center, FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 2);
                    } else {
                        cout << "错误:PnP求解失败" << endl;
                    }
            
            cv::imshow("Frame", frame);
        }

        int key = waitKey(1);
        if (key != -1) {
            onKeyPressed(key);
        }
    }
    cap.release();
    cv::destroyAllWindows();

    return 0;
}